#!/usr/bin/env python3

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal


def send_goal():
    pose_x, pose_y, pose_z = -4.0, 3.0, 0.0
    quat_x, quat_y, quat_z, quat_w = 0.0, 0.0, 0.0, 1.0

    client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    rospy.loginfo("Waiting for move_base action server...")
    client.wait_for_server()
    rospy.loginfo("Connected to move base server")

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = pose_x
    goal.target_pose.pose.position.y = pose_y
    goal.target_pose.pose.position.z = pose_z
    goal.target_pose.pose.orientation.x = quat_x
    goal.target_pose.pose.orientation.y = quat_y
    goal.target_pose.pose.orientation.z = quat_z
    goal.target_pose.pose.orientation.w = quat_w
    client.send_goal(goal)

    rospy.loginfo("\r\nSending goal \
                  \r\n Position \
                  \r\n  pose_x: %s, pose_y: %s pose_z: %s \
                  \r\n Orientation \
                  \r\n  quat_x: %s, quat_y: %s quat_z: %s, quat_w: %s" %
                  (pose_x, pose_y, pose_z,
                   quat_x, quat_y, quat_z, quat_w))

    res = client.wait_for_result()
    if res:
        rospy.loginfo("Mission complete!")
    else:
        rospy.logwarn("Mission failed ...")


if __name__ == '__main__':
    try:
        rospy.init_node('demo_simple_goal', anonymous=True)
        send_goal()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
